clc,clear
% 参数
n1=90;    % rpm
a=0.35;   % m
b=0.3;   % m
L1=0.08;   % m
L2=0.4;  % m
L3=0.2;  % m
L4=0.5;  % m
m2=5;     % kg
m4=24;    % kg
m5=40;    % kg
BS2=0.2; % m
CS4=0.3; % m
J2=0.01;  % kgm^2
J4=0.25;  % kgm^2
Pr=2000000;  % N

% 建立平面直角坐标系，求解
Ax=0;Ay=0;Dx=-b;Dy=a;Ey=a;
Equal_Divid_Theta1=0:2*pi/1000:2*pi;
for i=1:length(Equal_Divid_Theta1)
    Theta1=Equal_Divid_Theta1(i);
    Bx=L1*cos(Theta1);By=L1*sin(Theta1);
    BD=sqrt((Bx-Dx)^2+(By-Dy)^2);
    Theta_DBx=atan2(Dy-By,Dx-Bx);
    Theta_DBC=acos((BD^2+L2^2-L3^2)/(2*BD*L2));
    Theta2=Theta_DBx-Theta_DBC;
    Cx=Bx+L2*cos(Theta2);Cy=By+L2*sin(Theta2);
    Ex=Cx+sqrt(L4^2-(Cy-Dy)^2);
    % 质心位置
    S2x=Bx/2+Cx/2;
    S2y=By/2+Cy/2;
    S4x=Cx+3/5*(Ex-Cx);
    S4y=Ey+2/5*(Cy-Ey);
    Theta3=atan2(Cy-Dy,Cx-Dx);
    Theta4=atan2(Ey-Cy,Ex-Cx);
    % 速度
    A=[L2*sin(Theta2) L3*sin(Theta3) 0 0;...
       L2*cos(Theta2) L3*cos(Theta3) 0 0;...
       L2*sin(Theta2) 0 L4*sin(Theta4) 1;...
       L2*cos(Theta2) 0 L4*cos(Theta4) 0];
    w1=n1/60*2*pi; % 将转速换算为角速度 rad/s
    A_B=-[L1*sin(Theta1)*w1;L1*cos(Theta1)*w1;L1*sin(Theta1)*w1;L1*cos(Theta1)*w1];
    X=A\A_B;
    w2=X(1);w3=X(2);w4=X(3);vEx=X(4);
    % 质心速度
    vS2x=-w1*L1*sin(Theta1)-BS2*w2*sin(Theta2);
    vS2y=w1*L1*cos(Theta1)+BS2*w2*cos(Theta2);
    vS4x=-w3*L3*sin(Theta3)-CS4*w4*sin(Theta4);
    vS4y=w3*L3*cos(Theta3)+CS4*w4*cos(Theta4);
    % 矩阵法求加速度
    a1=0;
    A_B=-[ L1*cos(Theta1)*w1^2+L2*cos(Theta2)*w2^2+L3*cos(Theta3)*w3^2+L1*sin(Theta1)*a1;...
        -L1*sin(Theta1)*w1^2-L2*sin(Theta2)*w2^2-L3*sin(Theta3)*w3^2+L1*cos(Theta1)*a1;...
         L1*cos(Theta1)*w1^2+L2*cos(Theta2)*w2^2+L4*cos(Theta4)*w4^2+L1*sin(Theta1)*a1;...
        -L1*sin(Theta1)*w1^2-L2*sin(Theta2)*w2^2-L4*sin(Theta4)*w4^2+L1*cos(Theta1)*a1];
    X=A\A_B;
    a2=X(1);a3=X(2);a4=X(3);aEx=X(4);
    % 质心加速度
    aS2x=-L1*w1^2*cos(Theta1)-BS2*(w2^2*cos(Theta2)+a2*sin(Theta2));
    aS2y=-L1*w1^2*sin(Theta1)-BS2*(w2^2*sin(Theta2)-a2*cos(Theta2));
    aS4x=-L3*(w3^2*cos(Theta3)+a3*sin(Theta3))-CS4*(w4^2*cos(Theta4)+a4*sin(Theta4));
    aS4y=-L3*(w3^2*sin(Theta3)-a3*cos(Theta3))-CS4*(w4^2*sin(Theta4)-a4*cos(Theta4));
    % 质心惯性力
    fS2x=-m2*aS2x;
    fS2y=-m2*aS2y;
    fS4x=-m4*aS4x;
    fS4y=-m4*aS4y;
    M2=-J2*a2;
    M4=-J4*a4;
    fEx=-m5*aEx;
    % 受力起止点计算
    SP=pi-atan(a/b)-acos((a^2+b^2+(L1+L2)^2-L3^2)/(2*sqrt(a^2+b^2)*(L1+L2)))+20*pi/180;
    EP=pi/2+atan((b-L3)/a)+acos((L1^2+(b-L3)^2+a^2-L2^2)/(2*L1*sqrt((b-L3)^2+a^2)))-20*pi/180;
    % 根据外载荷组成方程组列阵
    if Theta1>SP && Theta1<EP
        A_B=[-fS2x -fS2y+m2*9.8 -M2 -fS4x -fS4y+m4*9.8 -M4 -fEx-Pr m5*9.8]';
    else
        A_B=[-fS2x -fS2y+m2*9.8 -M2 -fS4x -fS4y+m4*9.8 -M4 -fEx-0 m5*9.8]';
    end
    %平衡方程系数矩阵
    A=[1    0  -1   0   0  0   0   0;...
       0    1   0  -1   0  0   0   0;...
       (By-S2y) (S2x-Bx)  -(Cy-S2y) -(S2x-Cx)   0  0   0   0;...
       0    0   1   0   1  0  -1   0;...
       0    0   0   1   0  1   0   0;...
       0    0  (Cy-S4y) (S4x-Cx) (Cy-S4y) (S4x-Cx) -(Ey-S4y) -(S4x-Ex);...
       0    0   0   0   0  0   1   0;...
       0    0   0   0  (Cy-Dy) (Dx-Cx)   0   0];
    f=A\A_B;
    FAx=-f(1);
    FAy=-f(2);
    M=FAx*(Ay-By)+FAy*(Bx-Ax);
    Z=[FAx FAy f'];
    % 将所求结果存入矩阵
    WZ(i,:)=[Theta1 Theta2 Theta3 Theta4  Ex S2x S2y S4x S4y];
    SD(i,:)=[w1 w2 -w3 w4  vEx vS2x vS2y vS4x vS4y];
    JSD(i,:)=[a1 a2 -a3 a4  aEx aS2x aS2y aS4x aS4y];
    GXL(i,:)=[-fS2x -fS2y -M2 -fS4x -fS4y -M4 -fEx];
    YSL(i,:)=Z;
    PHLJ(i,:)=M;
end
% 曲线图
figure(1)
plot(WZ(:,1)/w1,WZ(:,6))
title('S2x')
xlabel('时间 s')
ylabel('m')
figure(2)
plot(WZ(:,1)/w1,WZ(:,7))
title('S2y')
xlabel('时间 s')
ylabel('m')
figure(3)
plot(WZ(:,1)/w1,WZ(:,8))
title('S4x')
xlabel('时间 s')
ylabel('m')
figure(4)
plot(WZ(:,1)/w1,WZ(:,9))
title('S4y')
xlabel('时间 s')
ylabel('m')
figure(5)
plot(WZ(:,1)/w1,SD(:,6))
title('vS2x')
xlabel('时间 s')
ylabel('m/s')
figure(6)
plot(WZ(:,1)/w1,SD(:,7))
title('vS2y')
xlabel('时间 s')
ylabel('m/s')
figure(7)
plot(WZ(:,1)/w1,SD(:,8))
title('vS4x')
xlabel('时间 s')
ylabel('m/s')
figure(8)
plot(WZ(:,1)/w1,SD(:,9))
title('vS4y')
xlabel('时间 s')
ylabel('m/s')
figure(9)
plot(WZ(:,1)/w1,JSD(:,6))
title('aS2x')
xlabel('时间 s')
ylabel('m/s^2')
figure(10)
plot(WZ(:,1)/w1,JSD(:,7))
title('aS2y')
xlabel('时间 s')
ylabel('m/s^2')
figure(11)
plot(WZ(:,1)/w1,JSD(:,8))
title('aS4x')
xlabel('时间 s')
ylabel('m/s^2')
figure(12)
plot(WZ(:,1)/w1,JSD(:,9))
title('aS4y')
xlabel('时间 s')
ylabel('m/s^2')
figure(13)
plot(WZ(:,1)/w1,GXL)
title('杆件惯性力')
xlabel('时间 s')
ylabel('N')
legend('杆2x','杆2y','杆2惯性矩','杆4x','杆4y','杆4惯性矩','滑块Ex')
figure(14)
plot(WZ(:,1)/w1,YSL)
title('支座约束反力')
xlabel('时间 s')
ylabel('N')
legend('Ax','Ay','Bx','By','Cx','Cy','Dx','Dy','Ex','Ey')
figure(15)
plot(WZ(:,1)/w1,WZ(:,5))
title('滑块位移')
xlabel('时间 s')
ylabel('m')
figure(16)
plot(WZ(:,1)/w1,SD(:,5))
title('滑块速度')
xlabel('时间 s')
ylabel('m/s')
figure(17)
plot(WZ(:,1)/w1,JSD(:,5))
title('滑块加速度')
xlabel('时间 s')
ylabel('m/s^2')
figure(18)
plot(WZ(:,1)/w1,WZ(:,2))
title('杆2转角')
xlabel('时间 s')
ylabel('rad')
figure(19)
plot(WZ(:,1)/w1,SD(:,2))
title('杆2角速度')
xlabel('时间 s')
ylabel('rad/s')
figure(20)
plot(WZ(:,1)/w1,JSD(:,2))
title('杆2角加速度')
xlabel('时间 s')
ylabel('rad/s^2')
figure(21)
plot(WZ(:,1)/w1,WZ(:,3))
title('杆3转角')
xlabel('时间 s')
ylabel('rad')
figure(22)
plot(WZ(:,1)/w1,SD(:,3))
title('杆3角速度')
xlabel('时间 s')
ylabel('rad/s')
figure(23)
plot(WZ(:,1)/w1,JSD(:,3))
title('杆3角加速度')
xlabel('时间 s')
ylabel('rad/s^2')
figure(24)
plot(WZ(:,1)/w1,WZ(:,4))
title('杆4转角')
xlabel('时间 s')
ylabel('rad')
figure(25)
plot(WZ(:,1)/w1,SD(:,4))
title('杆4角速度')
xlabel('时间 s')
ylabel('rad/s')
figure(26)
plot(WZ(:,1)/w1,JSD(:,4))
title('杆4角加速度')
xlabel('时间 s')
ylabel('rad/s^2')
figure(27)
plot(WZ(:,1)/w1,PHLJ)
title('平衡力矩')
xlabel('时间 s')
ylabel('Nm')






% 12个位置的角度分别为

TT=[0 2*pi/8  2*pi/8*2  2*pi/8*3  2*pi/8*4  2*pi/8*5  2*pi/8*6  2*pi/8*7  SP-20/180*pi  EP+20/180*pi SP EP SP EP ];
for i=1:14
    Theta1=TT(i);
    Bx=L1*cos(Theta1);By=L1*sin(Theta1);
    BD=sqrt((Bx-Dx)^2+(By-Dy)^2);
    Theta_DBx=atan2(Dy-By,Dx-Bx);
    Theta_DBC=acos((BD^2+L2^2-L3^2)/(2*BD*L2));
    Theta2=Theta_DBx-Theta_DBC;
    Cx=Bx+L2*cos(Theta2);Cy=By+L2*sin(Theta2);
    Ex=Cx+sqrt(L4^2-(Cy-Dy)^2);
    % 质心位置
    S2x=Bx/2+Cx/2;
    S2y=By/2+Cy/2;
    S4x=Cx+3/5*(Ex-Cx);
    S4y=Ey+2/5*(Cy-Ey);
    Theta3=atan2(Cy-Dy,Cx-Dx);
    Theta4=atan2(Ey-Cy,Ex-Cx);
    % 速度
    A=[L2*sin(Theta2) L3*sin(Theta3) 0 0;...
       L2*cos(Theta2) L3*cos(Theta3) 0 0;...
       L2*sin(Theta2) 0 L4*sin(Theta4) 1;...
       L2*cos(Theta2) 0 L4*cos(Theta4) 0];
    A_B=-[L1*sin(Theta1)*w1;L1*cos(Theta1)*w1;L1*sin(Theta1)*w1;L1*cos(Theta1)*w1];
    X=A\A_B;
    w2=X(1);w3=X(2);w4=X(3);vEx=X(4);
    % 质心速度
    vS2x=-w1*L1*sin(Theta1)-BS2*w2*sin(Theta2);
    vS2y=w1*L1*cos(Theta1)+BS2*w2*cos(Theta2);
    vS4x=-w3*L3*sin(Theta3)-CS4*w4*sin(Theta4);
    vS4y=w3*L3*cos(Theta3)+CS4*w4*cos(Theta4);
    % 矩阵法求加速度
    a1=0;
    A_B=-[ L1*cos(Theta1)*w1^2+L2*cos(Theta2)*w2^2+L3*cos(Theta3)*w3^2+L1*sin(Theta1)*a1;...
        -L1*sin(Theta1)*w1^2-L2*sin(Theta2)*w2^2-L3*sin(Theta3)*w3^2+L1*cos(Theta1)*a1;...
         L1*cos(Theta1)*w1^2+L2*cos(Theta2)*w2^2+L4*cos(Theta4)*w4^2+L1*sin(Theta1)*a1;...
        -L1*sin(Theta1)*w1^2-L2*sin(Theta2)*w2^2-L4*sin(Theta4)*w4^2+L1*cos(Theta1)*a1];
    X=A\A_B;
    a2=X(1);a3=X(2);a4=X(3);aEx=X(4);
    % 质心加速度
    aS2x=-L1*w1^2*cos(Theta1)-BS2*(w2^2*cos(Theta2)+a2*sin(Theta2));
    aS2y=-L1*w1^2*sin(Theta1)-BS2*(w2^2*sin(Theta2)-a2*cos(Theta2));
    aS4x=-L3*(w3^2*cos(Theta3)+a3*sin(Theta3))-CS4*(w4^2*cos(Theta4)+a4*sin(Theta4));
    aS4y=-L3*(w3^2*sin(Theta3)-a3*cos(Theta3))-CS4*(w4^2*sin(Theta4)-a4*cos(Theta4));
    % 质心惯性力
    fS2x=-m2*aS2x;
    fS2y=-m2*aS2y;
    fS4x=-m4*aS4x;
    fS4y=-m4*aS4y;
    M2=-J2*a2;
    M4=-J4*a4;
    fEx=-m5*aEx;
    % 受力起止点计算
    SP=pi-atan(a/b)-acos((a^2+b^2+(L1+L2)^2-L3^2)/(2*sqrt(a^2+b^2)*(L1+L2)))+20*pi/180;
    EP=pi/2+atan((b-L3)/a)+acos((L1^2+(b-L3)^2+a^2-L2^2)/(2*L1*sqrt((b-L3)^2+a^2)))-20*pi/180;
    % 根据外载荷组成方程组列阵
    if Theta1>SP && Theta1<EP
       A_B=[-fS2x -fS2y+m2*9.8 -M2 -fS4x -fS4y+m4*9.8 -M4 -fEx-Pr m5*9.8]';
    else
        A_B=[-fS2x -fS2y+m2*9.8 -M2 -fS4x -fS4y+m4*9.8 -M4 -fEx-0 m5*9.8]';
    end
    if i==11
        A_B=[-fS2x -fS2y+m2*9.8 -M2 -fS4x -fS4y+m4*9.8 -M4 -fEx-Pr m5*9.8]';
    elseif i==12
        A_B=[-fS2x -fS2y+m2*9.8 -M2 -fS4x -fS4y+m4*9.8 -M4 -fEx-0 m5*9.8]';
    elseif i==13
        A_B=[-fS2x -fS2y+m2*9.8 -M2 -fS4x -fS4y+m4*9.8 -M4 -fEx-Pr m5*9.8]';
    elseif i==14
        A_B=[-fS2x -fS2y+m2*9.8 -M2 -fS4x -fS4y+m4*9.8 -M4 -fEx-0 m5*9.8]';
    end
    %平衡方程系数矩阵
    A=[1    0  -1   0   0  0   0   0;...
       0    1   0  -1   0  0   0   0;...
       (By-S2y) (S2x-Bx)  -(Cy-S2y) -(S2x-Cx)   0  0   0   0;...
       0    0   1   0   1  0  -1   0;...
       0    0   0   1   0  1   0   0;...
       0    0  (Cy-S4y) (S4x-Cx) (Cy-S4y) (S4x-Cx) -(Ey-S4y) -(S4x-Ex);...
       0    0   0   0   0  0   1   0;...
       0    0   0   0  (Cy-Dy) (Dx-Cx)   0   0];
    f=A\A_B;
    FAx=-f(1);
    FAy=-f(2);
    M=FAx*(Ay-By)+FAy*(Bx-Ax);
    Z=[FAx FAy f'];
    % 将所求结果存入矩阵
    WZ(i,:)=[Theta1 Theta2 Theta3 Theta4  Ex S2x S2y S4x S4y];
    SD(i,:)=[w1 w2 -w3 w4  vEx vS2x vS2y vS4x vS4y];
    JSD(i,:)=[a1 a2 -a3 a4  aEx aS2x aS2y aS4x aS4y];
    GXL(i,:)=[-fS2x -fS2y -M2 -fS4x -fS4y -M4 -fEx];
    YSL(i,:)=Z;
    PHLJ(i,:)=M;
    % 展示结果
    if i==11
        disp(['————————   位置',num2str(10),'（阻力为Pr）： ——————'])
    elseif i==12
        disp(['——————————   位置',num2str(10),'： ——————————'])
    elseif i==13
        disp(['————————   位置',num2str(11),'（阻力为Pr）： ——————'])
    elseif i==14
        disp(['——————————   位置',num2str(11),'： ——————————'])
    else
        disp(['——————————   位置',num2str(i-1),'： ——————————'])
    end
    disp(['位置信息：（Theta1 Theta2 Theta3 Theta4  Ex S2x S2y S4x S4y）：',num2str(WZ(i,:))])
    disp(['速度信息：（w1 w2 w3 w4  vEx vS2x vS2y vS4x vS4y）：',num2str(SD(i,:))])
    disp(['加速度信息：（a1 a2 a3 a4  aEx aS2x aS2y aS4x aS4y）：',num2str(JSD(i,:))])
    disp(['惯性力信息：（-fS2x -fS2y -M2 -fS4x -fS4y -M4 -fEx）：',num2str(GXL(i,:))])
    disp(['约束力信息：（FAx FAy FBx FBy FCx FCy FDx FDy FEx FEy）：',num2str(YSL(i,:))])
    disp(['平衡力矩信息：',num2str(PHLJ(i,:))])

end


运行结果
——————————   位置0： ——————————
位置信息：（Theta1 Theta2 Theta3 Theta4  Ex S2x S2y S4x S4y）：0      2.0379    0.035743   -0.014295     0.39982   -0.010064     0.17857     0.19984     0.35286
速度信息：（w1 w2 w3 w4  vEx vS2x vS2y vS4x vS4y）：9.4248    0.074152      3.7055     -1.4814   -0.037071   -0.013242      0.7473    0.020131      -1.185
加速度信息：（a1 a2 a3 a4  aEx aS2x aS2y aS4x aS4y）：0     -27.0996      23.9217     -9.79122      1.40627     -2.26635      2.43971      -3.2737     -7.80708
惯性力信息：（-fS2x -fS2y -M2 -fS4x -fS4y -M4 -fEx）：-11.33175      12.19856    -0.2709959     -78.56876     -187.3699     -2.447804      56.25085
约束力信息：（FAx FAy FBx FBy FCx FCy FDx FDy FEx FEy）：1015.9097     -2035.1577     -1015.9097      2035.1577      -1004.578      1973.9592      982.26006      -1926.129       56.25085     -84.311007
平衡力矩信息：-162.8126
——————————   位置1： ——————————
位置信息：（Theta1 Theta2 Theta3 Theta4  Ex S2x S2y S4x S4y）：0.7854      2.0067     0.35308    -0.13876     0.38286   -0.027884     0.23786     0.18478     0.37766
速度信息：（w1 w2 w3 w4  vEx vS2x vS2y vS4x vS4y）：9.4248    -0.79248      3.5543     -1.3469    -0.33895    -0.38947     0.60007     0.18991     -1.0672
加速度信息：（a1 a2 a3 a4  aEx aS2x aS2y aS4x aS4y）：0     -22.4723     -12.4182      2.68848      2.51701    -0.897642     -3.24079     -3.65699      2.33086
惯性力信息：（-fS2x -fS2y -M2 -fS4x -fS4y -M4 -fEx）：-4.48821     -16.20393    -0.2247229     -87.76772      55.94074     0.6721203      100.6806
约束力信息：（FAx FAy FBx FBy FCx FCy FDx FDy FEx FEy）：952.91247     -2058.5199     -952.91247      2058.5199     -948.42426      2025.7238      961.33711     -1734.5831      100.68057     -450.08375
平衡力矩信息：-170.3523
——————————   位置2： ——————————
位置信息：（Theta1 Theta2 Theta3 Theta4  Ex S2x S2y S4x S4y）：1.5708      1.9058     0.56902    -0.21723     0.35673   -0.065757     0.26888     0.16144      0.3931
速度信息：（w1 w2 w3 w4  vEx vS2x vS2y vS4x vS4y）：9.4248     -1.6324      1.2742    -0.43971     -0.1847    -0.44565     0.10734     0.10888     -0.3435
加速度信息：（a1 a2 a3 a4  aEx aS2x aS2y aS4x aS4y）：0     -11.3101     -40.3614      13.5269      5.98626      2.31149     -6.86573     -3.80501      10.6006
惯性力信息：（-fS2x -fS2y -M2 -fS4x -fS4y -M4 -fEx）：11.55744     -34.32866    -0.1131007     -91.32013      254.4134      3.381732      239.4505
约束力信息：（FAx FAy FBx FBy FCx FCy FDx FDy FEx FEy）：769.92373     -2236.3224     -769.92373      2236.3224     -781.48117       2221.651      929.61152     -1732.0376      239.45048      -818.9946
平衡力矩信息：-61.5939
——————————   位置3： ——————————
位置信息：（Theta1 Theta2 Theta3 Theta4  Ex S2x S2y S4x S4y）：2.3562      1.7478     0.52546    -0.20202     0.36285   -0.091775     0.25345     0.16692     0.39013
速度信息：（w1 w2 w3 w4  vEx vS2x vS2y vS4x vS4y）：9.4248     -1.9381     -2.2927     0.80982     0.31125    -0.15157    -0.46491    -0.18126     0.63468
加速度信息：（a1 a2 a3 a4  aEx aS2x aS2y aS4x aS4y）：0     0.696962     -40.9221      13.2437      6.02225      5.01982     -5.78887     -4.41041      10.4847
惯性力信息：（-fS2x -fS2y -M2 -fS4x -fS4y -M4 -fEx）：25.09908     -28.94435    0.00696962     -105.8499      251.6336      3.310914        240.89
约束力信息：（FAx FAy FBx FBy FCx FCy FDx FDy FEx FEy）：188316.2552     -1053158.0865      -188316.2552      1053158.0865     -188341.35429      1053138.0308     -1811523.6056     -1052651.1972       -1999759.11      1023243.8618
平衡力矩信息：48922.8419
——————————   位置4： ——————————
位置信息：（Theta1 Theta2 Theta3 Theta4  Ex S2x S2y S4x S4y）：3.1416      1.6362     0.24827    -0.09845     0.39145   -0.093066     0.19957     0.19242     0.36966
速度信息：（w1 w2 w3 w4  vEx vS2x vS2y vS4x vS4y）：9.4248    -0.47104     -3.8257      1.4906     0.26127    0.094007    -0.74783    -0.14406      1.1867
加速度信息：（a1 a2 a3 a4  aEx aS2x aS2y aS4x aS4y）：0      10.0297      -5.5189     0.485288      2.02696      5.10737     -0.17533     -3.75762     0.561055
惯性力信息：（-fS2x -fS2y -M2 -fS4x -fS4y -M4 -fEx）：25.5368    -0.876649     0.100297     -90.1828      13.4653     0.121322      81.0784
约束力信息：（FAx FAy FBx FBy FCx FCy FDx FDy FEx FEy）：32786.763336      -501003.0301     -32786.763336       501003.0301     -32812.300173      500954.90675     -1967196.8043     -500706.24142     -1999918.9216      493467.01426
平衡力矩信息：40080.2424
——————————   位置5： ——————————
位置信息：（Theta1 Theta2 Theta3 Theta4  Ex S2x S2y S4x S4y）：3.927      1.6801   -0.044792    0.017912     0.39972   -0.078385     0.14224     0.19975     0.34642
速度信息：（w1 w2 w3 w4  vEx vS2x vS2y vS4x vS4y）：9.4248      1.4079     -2.9759      1.1893   -0.037301     0.25325    -0.56386    0.020259     0.95132
加速度信息：（a1 a2 a3 a4  aEx aS2x aS2y aS4x aS4y）：0       7.9575      19.8637     -7.75481      1.30955      3.48602      4.45712     -2.32988     -6.22312
惯性力信息：（-fS2x -fS2y -M2 -fS4x -fS4y -M4 -fEx）：17.4301      22.28558    0.07957505     -55.91718     -149.3548     -1.938701      52.38214
约束力信息：（FAx FAy FBx FBy FCx FCy FDx FDy FEx FEy）：221.26961     -2129.6391     -221.26961      2129.6391     -238.69971      2058.3535      235.16468     -1972.5083      52.382141     -137.61946
平衡力矩信息：132.9875
——————————   位置6： ——————————
位置信息：（Theta1 Theta2 Theta3 Theta4  Ex S2x S2y S4x S4y）：4.7124      1.8361    -0.22181    0.088114     0.39316    -0.05245       0.113     0.19394      0.3324
速度信息：（w1 w2 w3 w4  vEx vS2x vS2y vS4x vS4y）：9.4248      2.0809     -1.1188     0.43826   -0.068512     0.35238    -0.10914    0.037658     0.34925
加速度信息：（a1 a2 a3 a4  aEx aS2x aS2y aS4x aS4y）：0     -2.83738      29.6641     -11.4925      1.95944      0.77472      6.41925     -1.30344     -9.17183
惯性力信息：（-fS2x -fS2y -M2 -fS4x -fS4y -M4 -fEx）：3.8736      32.09627   -0.02837376     -31.28251     -220.1238     -2.873116      78.37774
约束力信息：（FAx FAy FBx FBy FCx FCy FDx FDy FEx FEy）：598.97146     -2251.9769     -598.97146      2251.9769     -602.84506      2170.8807      649.94029     -2155.8045      78.377744     -23.870923
平衡力矩信息：47.9177
——————————   位置7： ——————————
位置信息：（Theta1 Theta2 Theta3 Theta4  Ex S2x S2y S4x S4y）：5.4978      1.9841    -0.20264     0.08059     0.39428   -0.023762     0.12659     0.19493      0.3339
速度信息：（w1 w2 w3 w4  vEx vS2x vS2y vS4x vS4y）：9.4248      1.2709      1.6792    -0.66006    0.094157     0.30037     0.43105   -0.051648    -0.52634
加速度信息：（a1 a2 a3 a4  aEx aS2x aS2y aS4x aS4y）：0     -18.7569      38.5901     -14.9065      2.48857     -1.45955      6.23569     -1.87596     -11.9146
惯性力信息：（-fS2x -fS2y -M2 -fS4x -fS4y -M4 -fEx）：-7.297732      31.17845     -0.187569     -45.02311     -285.9494     -3.726627      99.54267
约束力信息：（FAx FAy FBx FBy FCx FCy FDx FDy FEx FEy）：967.48133     -2238.8551     -967.48133      2238.8551      -960.1836      2158.6767      1014.7032     -2209.4261      99.542667      72.074862
平衡力矩信息：-71.9198
——————————   位置8： ——————————
位置信息：（Theta1 Theta2 Theta3 Theta4  Ex S2x S2y S4x S4y）：1.8529      1.8529     0.58848    -0.22391     0.35388   -0.077959     0.26893     0.15887     0.39441
速度信息：（w1 w2 w3 w4  vEx vS2x vS2y vS4x vS4y）：9.4248      -1.885           0           0           0    -0.36208    -0.10496           0           0
加速度信息：（a1 a2 a3 a4  aEx aS2x aS2y aS4x aS4y）：0     -6.74271     -44.7185      15.2594      6.65874      3.47158     -7.13217     -3.94819      11.9028
惯性力信息：（-fS2x -fS2y -M2 -fS4x -fS4y -M4 -fEx）：17.3579     -35.66087   -0.06742711     -94.75662      285.6661      3.814839      266.3497
约束力信息：（FAx FAy FBx FBy FCx FCy FDx FDy FEx FEy）：662.44354     -2322.3998     -662.44354      2322.3998     -679.80144      2309.0607      851.39454     -1788.1946      266.34972     -881.00501
平衡力矩信息：0.8289
——————————   位置9： ——————————
位置信息：（Theta1 Theta2 Theta3 Theta4  Ex S2x S2y S4x S4y）：3.7906      1.6616           0           0         0.4   -0.081867     0.15082         0.2        0.35
速度信息：（w1 w2 w3 w4  vEx vS2x vS2y vS4x vS4y）：9.4248       1.144     -3.2108      1.2843           0     0.22786    -0.62142           0      1.0275
加速度信息：（a1 a2 a3 a4  aEx aS2x aS2y aS4x aS4y）：0       9.1548      17.2082      -6.8833      1.23714      3.86158      3.86832     -2.55675     -5.50664
惯性力信息：（-fS2x -fS2y -M2 -fS4x -fS4y -M4 -fEx）：19.3079      19.34159    0.09154796     -61.36194     -132.1593     -1.720824      49.48544
约束力信息：（FAx FAy FBx FBy FCx FCy FDx FDy FEx FEy）：181.50569     -2131.3823     -181.50569      2131.3823     -200.81359      2063.0407      188.93709          -1960      49.485438     -163.16515
平衡力矩信息：144.6179
————————   位置10（阻力为Pr）： ——————
位置信息：（Theta1 Theta2 Theta3 Theta4  Ex S2x S2y S4x S4y）：2.202      1.7801     0.55776    -0.21333     0.35835   -0.088761     0.26022     0.16289     0.39234
速度信息：（w1 w2 w3 w4  vEx vS2x vS2y vS4x vS4y）：9.4248     -2.0001     -1.6426      0.5704     0.23427    -0.21741    -0.36184    -0.13766     0.44598
加速度信息：（a1 a2 a3 a4  aEx aS2x aS2y aS4x aS4y）：0     -1.50155     -43.9801       14.617       6.5018       4.6535     -6.45706     -4.28048      11.4837
惯性力信息：（-fS2x -fS2y -M2 -fS4x -fS4y -M4 -fEx）：23.26749      -32.2853   -0.01501552     -102.7314      275.6079       3.65426      260.0721
约束力信息：（FAx FAy FBx FBy FCx FCy FDx FDy FEx FEy）：234484.93145     -1104110.6963     -234484.93145      1104110.6963     -234508.19894      1104093.9816     -1765334.4604     -1103583.1738     -1999739.9279      1082270.7325
平衡力矩信息：36981.2417
——————————   位置10： ——————————
位置信息：（Theta1 Theta2 Theta3 Theta4  Ex S2x S2y S4x S4y）：3.4415      1.6339     0.12818   -0.051155     0.39771   -0.089034     0.17596     0.19797     0.36023
速度信息：（w1 w2 w3 w4  vEx vS2x vS2y vS4x vS4y）：9.4248     0.32288     -3.6724      1.4588     0.13119     0.15834    -0.72439   -0.071512      1.1655
加速度信息：（a1 a2 a3 a4  aEx aS2x aS2y aS4x aS4y）：0      10.7766      7.26748     -3.68639      1.33246      4.63911      1.94304     -3.18356      -2.8582
惯性力信息：（-fS2x -fS2y -M2 -fS4x -fS4y -M4 -fEx）：23.1955      9.71519     0.107766     -76.4054     -68.5968    -0.921598      53.2983
约束力信息：（FAx FAy FBx FBy FCx FCy FDx FDy FEx FEy）：124.84479      -2185.429     -124.84479       2185.429     -148.04034      2126.7138      124.93324     -1960.1106      53.298285     -255.47307
平衡力矩信息：169.979
————————   位置11（阻力为Pr）： ——————
位置信息：（Theta1 Theta2 Theta3 Theta4  Ex S2x S2y S4x S4y）：2.202      1.7801     0.55776    -0.21333     0.35835   -0.088761     0.26022     0.16289     0.39234
速度信息：（w1 w2 w3 w4  vEx vS2x vS2y vS4x vS4y）：9.4248     -2.0001     -1.6426      0.5704     0.23427    -0.21741    -0.36184    -0.13766     0.44598
加速度信息：（a1 a2 a3 a4  aEx aS2x aS2y aS4x aS4y）：0     -1.50155     -43.9801       14.617       6.5018       4.6535     -6.45706     -4.28048      11.4837
惯性力信息：（-fS2x -fS2y -M2 -fS4x -fS4y -M4 -fEx）：23.26749      -32.2853   -0.01501552     -102.7314      275.6079       3.65426      260.0721
约束力信息：（FAx FAy FBx FBy FCx FCy FDx FDy FEx FEy）：234484.93145     -1104110.6963     -234484.93145      1104110.6963     -234508.19894      1104093.9816     -1765334.4604     -1103583.1738     -1999739.9279      1082270.7325
平衡力矩信息：36981.2417
——————————   位置11： ——————————
位置信息：（Theta1 Theta2 Theta3 Theta4  Ex S2x S2y S4x S4y）：3.4415      1.6339     0.12818   -0.051155     0.39771   -0.089034     0.17596     0.19797     0.36023
速度信息：（w1 w2 w3 w4  vEx vS2x vS2y vS4x vS4y）：9.4248     0.32288     -3.6724      1.4588     0.13119     0.15834    -0.72439   -0.071512      1.1655
加速度信息：（a1 a2 a3 a4  aEx aS2x aS2y aS4x aS4y）：0      10.7766      7.26748     -3.68639      1.33246      4.63911      1.94304     -3.18356      -2.8582
惯性力信息：（-fS2x -fS2y -M2 -fS4x -fS4y -M4 -fEx）：23.1955      9.71519     0.107766     -76.4054     -68.5968    -0.921598      53.2983
约束力信息：（FAx FAy FBx FBy FCx FCy FDx FDy FEx FEy）：124.84479      -2185.429     -124.84479       2185.429     -148.04034      2126.7138      124.93324     -1960.1106      53.298285     -255.47307
平衡力矩信息：169.979

                           
